
import math
from Box2D import *

class Oscillator:
	def __init__(self):
		self.frequency = 0.0
		self.phase = 0.0
	
	def step(self):
		self.phase += (self.frequency) / 12.0
	
	def get_amplitude(self):
		return 2.0 #math.cos(self.phase)
		
	def set_frequency(self, frequency):
		self.frequency = frequency * 15 + 1
		
class Brain:
	
	def __init__(self, net):
		self.net = net
		
		self.knee_1 = 0.0
		self.knee_2 = 0.0
		self.hip_1 = 0.0
		self.hip_2 = 0.0
		
		self.oscillators = [Oscillator(), Oscillator()]

	def step(self, robot, reference):
		pos = (0,0,0) #robot.torso.getPosition()
		
		footpos_1 = (0,0,0) #moveToWorld((0,-0.2), robot.lower_leg_1)		
		footpos_2 = (0,0,0) #moveToWorld((0,-0.2), robot.lower_leg_2)
		
		ref = (reference.position_x - pos[0]) * 10
		cog = (robot.get_center_of_gravity()[0] - pos[0]) * 10
		
		ang = (0,0,0) #robot.torso.GetAngle()
		
		self.oscillators[0].step()
		self.oscillators[1].step()
		
		inputs = [0,0,0,0,
			      footpos_1[0], footpos_1[1], 
			      footpos_2[0], footpos_2[1], 
			      ref, pos[1], ang, cog,
			      self.oscillators[0].get_amplitude()]
			      
		action = self.net.pactivate(inputs)
		self.knee_1 = action[0] * 40.0 - 20.0
		self.knee_2 = action[1] * 40.0 - 20.0
		self.hip_1 = action[2] * 40.0 - 20.0
		self.hip_2 = action[3] * 40.0 - 20.0
		self.oscillators[0].set_frequency(action[4])
		self.oscillators[1].set_frequency(action[5])
